// --------------------------------------------------------------------------------------------------------------------
// <copyright file="KinectProcessorTest.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------
namespace Test.Robotics.Runtime
{
    using System;
    using System.Drawing;
    using System.IO;
    using System.Reflection;
    using System.Runtime.Serialization;
    using System.Threading;
    using Microsoft.Robotics.Numerics;
    using Microsoft.Robotics.Runtime;
    using Microsoft.Robotics.Vision;
    using Microsoft.Robotics.Vision.Cameras;
    using Microsoft.Robotics.Vision.Runtime.Cameras;
    using Microsoft.VisualStudio.TestTools.UnitTesting;
    using Test.Robotics.Runtime;

    /// <summary>
    /// Validates that message synchronization works as expected.
    /// </summary>
    [TestClass]
    public class KinectProcessorTest
    {
        /// <summary>
        /// Send the KinectProcessor a fake raw kinect depth image, then validate the processed image meets the following:
        /// 1) Pixel are linearly scaled
        /// 2) Flipped Left to right
        /// 3) Out of range pixels are clamped
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void KinectProcessorDepthTest()
        {
            const int ImageHeight = 60;
            const int ImageWidth = 80;

            // Parameters of the fake input message
            long originatingTime = 10;
            Size depthImageSize = new Size(ImageWidth, ImageHeight); 
            double fieldOfView = 55;
            short maximumRange = 10000;
            short minimumRange = 100; 
            short furtherThanMaxValue = -32002; 
            short nearerThanMinValue = -32003;
            short noReadingValue = -32000;
            Pose pose = new Pose();
            short[] depthImage = this.CreateDepthImage(depthImageSize, maximumRange, noReadingValue);
            bool disableDepthMirror = true;

            this.RunSingleTest(
                originatingTime,
                depthImageSize,
                fieldOfView,
                maximumRange,
                minimumRange,
                furtherThanMaxValue,
                nearerThanMinValue,
                noReadingValue,
                pose,
                depthImage,
                disableDepthMirror);
        }

        /// <summary>
        /// Send the KinectProcessor a fake raw kinect depth image, then validate the processed image meets the following:
        /// 1) Pixel are linearly scaled
        /// 2) Flipped Left to right
        /// 3) Out of range pixels are clamped
        /// This test is same as KinectProcessorDepthTest except test case when depth image is flipped
        /// Can make this test priority 1
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void KinectProcessorDepthTestFlipped()
        {
            // Parameters of the fake input message
            long originatingTime = 10;
            Size depthImageSize = new Size(80, 60);
            double fieldOfView = 55;
            short maximumRange = 10000;
            short minimumRange = 100;
            short furtherThanMaxValue = -32002;
            short nearerThanMinValue = -32003;
            short noReadingValue = -32000;
            Pose pose = new Pose();
            short[] depthImage = this.CreateDepthImage(depthImageSize, maximumRange, noReadingValue);
            bool disableDepthMirror = false;

            this.RunSingleTest(
                originatingTime,
                depthImageSize,
                fieldOfView,
                maximumRange,
                minimumRange,
                furtherThanMaxValue,
                nearerThanMinValue,
                noReadingValue,
                pose,
                depthImage,
                disableDepthMirror);
        }

        /// <summary>
        /// Runs a single test with a set of parameters
        /// </summary>
        /// <param name="originatingTime">Fake message originating time</param>
        /// <param name="depthImageSize">depth image sixe</param>
        /// <param name="fieldOfView">Field of view</param>
        /// <param name="maximumRange">Maximum range</param>
        /// <param name="minimumRange">Minimum range</param>
        /// <param name="furtherThanMaxValue">Kinect further than maximum</param>
        /// <param name="nearerThanMinValue">Kinect nearer than maximum</param>
        /// <param name="noReadingValue">No reading value</param>
        /// <param name="pose">Kinect pose</param>
        /// <param name="depthImage">Sample depth image</param>
        /// <param name="disableDepthMirror">Flag to disable depth image flipping</param>
        private void RunSingleTest(
            long originatingTime,
            Size depthImageSize,
            double fieldOfView,
            short maximumRange,
            short minimumRange,
            short furtherThanMaxValue,
            short nearerThanMinValue,
            short noReadingValue,
            Pose pose,
            short[] depthImage,
            bool disableDepthMirror)
    {
            // Create a pipeline and send a single Kinect message
            AgentLocator producerLocator = new AgentLocator();
            MessageInjectorAgent<DepthCameraAgentMessage<KinectDepthCameraParameters>> raw = new MessageInjectorAgent<DepthCameraAgentMessage<KinectDepthCameraParameters>>("raw");
            producerLocator.Register(raw);

            MockCalibrationFileHelper mockCorrectionHelper = new MockCalibrationFileHelper();
            MockFloorFilter mockFoorFilter = new MockFloorFilter();
            KinectCameraState cameraState = new KinectCameraState(false, false);

            KinectAgentMessageProcessor processor = new KinectAgentMessageProcessor(
                mockCorrectionHelper,
                mockFoorFilter, 
                "processor",
                "dummy_file_name_that_will_be_ignored_because_mock_processor_has_hardcoded_coeffs", 
                disableDepthMirror,
                raw);

            producerLocator.Register(processor);

            MessageCollectorAgent<DepthCameraAgentMessage<KinectDepthCameraParameters>> collector = new MessageCollectorAgent<DepthCameraAgentMessage<KinectDepthCameraParameters>>("collector", processor);
            IAgentHost collectorHost = producerLocator.Register(collector);
            collectorHost.Activate();

            ImageFrameDepth depthFrame = new ImageFrameDepth(depthImageSize.Width, depthImageSize.Height);
            Array.Copy(depthImage, depthFrame.ImageData, depthImage.Length);

            KinectDepthCameraParameters kinectParams = new KinectDepthCameraParameters(
                CameraParameters.CreateIntrinsics(525, 525, depthFrame.Width / 2, depthFrame.Height / 2),
                new LensDistortion(),
                fieldOfView * MathConstants.Degrees2Radians / 2.0f,
                fieldOfView * MathConstants.Degrees2Radians / 2.0f,
                Microsoft.Kinect.DepthImageFormat.Resolution640x480Fps30,
                maximumRange,
                minimumRange,
                new Microsoft.Kinect.DepthRange(),
                furtherThanMaxValue,
                nearerThanMinValue,
                noReadingValue,
                false,
                null,
                null);

            DepthCameraAgentMessage<KinectDepthCameraParameters> kinectMessage = new DepthCameraAgentMessage<KinectDepthCameraParameters>(
                originatingTime,
                depthFrame,
                kinectParams);

            raw.Publish(kinectMessage);

            // Wait for the message to appear at end of pipeline
            collector.Wait(250);

            Assert.IsTrue(
                collector.Results.Count == 1, 
                string.Format("Expected 1 message at end of pipeline found {0}", collector.Results.Count));
             
            foreach (DepthCameraAgentMessage<KinectDepthCameraParameters> procMsg in collector.Results)
            {
                Assert.IsTrue(procMsg.OriginatingTime == originatingTime, string.Format("Transformed image originating time stamp {0} != {1}", procMsg.OriginatingTime, originatingTime));
                double expectedFOV = fieldOfView / 2 * MathConstants.Degrees2Radians;
                Assert.IsTrue(
                    this.IsDoubleEqual(procMsg.Parameters.HalfHorizontalFieldOfViewInRadians,  expectedFOV),
                    string.Format(
                    "Transformed image field of view {0} is not expected  value {1} (half  original field of view {2} converted to radians)",
                    procMsg.Parameters.HalfHorizontalFieldOfViewInRadians, 
                    expectedFOV, 
                    fieldOfView));

                this.VerifyBaseToProcessedDepthImage(depthImage, procMsg, disableDepthMirror, mockCorrectionHelper, mockFoorFilter);
            }

            collectorHost.Deactivate();
        }

        /// <summary>
        /// Create a random depth image. About 10% of pixels are set
        /// to noReading and 10% are greater than the maximumDepth
        /// </summary>
        /// <param name="requestSize">Size of created image</param>
        /// <param name="maxDepth">Maximum depth - approximately 10% will exceed this value</param>
        /// <param name="noReading">The magic no reading depth value approximately 10% are assigned this value</param>
        /// <returns>created image</returns>
        private short[] CreateDepthImage(Size requestSize, short maxDepth, short noReading)
        {
            Random rand = new Random(6930);
            short[] depthImage = new short[requestSize.Width * requestSize.Height];

            int maxVal = (int)(1.1 * maxDepth);

            for (int i = 0; i < depthImage.Length; ++i)
            {
                if (rand.Next(100) < 10)
                {
                    depthImage[i] = noReading;
                }
                else
                {
                    depthImage[i] = (short)rand.Next(maxVal);
                }
            }

            return depthImage;
        }

        /// <summary>
        /// Compares a base kinect depth image and its transformed version to verify the transformation
        /// </summary>
        /// <param name="baseImage">Input or base image</param>
        /// <param name="procMsg">Transformed or processed image</param>
        /// <param name="disableDepthMirror">Flag indicating if depth image should be flipped left to right</param>
        /// <param name="mockCorrectionHelper">Mock correction helper</param>
        /// <param name="floorFilter">Mock floor filter</param>
        private void VerifyBaseToProcessedDepthImage(
            short[] baseImage,
            DepthCameraAgentMessage<KinectDepthCameraParameters> procMsg,
            bool disableDepthMirror,
            MockCalibrationFileHelper mockCorrectionHelper,
            MockFloorFilter floorFilter)
        {
            Assert.IsTrue(
                baseImage.Length == procMsg.ImageFrame.Width * procMsg.ImageFrame.Height,
                string.Format(
                "Base image length {0} !=  transformed image size of {1} * {2} = {3}",
                baseImage.Length,
                procMsg.ImageFrame.Width,
                procMsg.ImageFrame.Height,
                procMsg.ImageFrame.Width * procMsg.ImageFrame.Height));

            short[] procImage = procMsg.ImageFrame.ImageData;

            for (int row = 0; row < procMsg.ImageFrame.Height; row++)
            {
                for (int col = 0; col < procMsg.ImageFrame.Width; col++)
                {
                    int baseIndex = row * procMsg.ImageFrame.Width + col;
                    int procIndex = row * procMsg.ImageFrame.Width + procMsg.ImageFrame.Width - 1 - col;

                    if (true == disableDepthMirror)
                    {
                        procIndex = baseIndex;
                    }

                    short rawValue = baseImage[baseIndex];
                    short correctedValue = procImage[procIndex];

                    short transformedValue = this.GetCorrectedDepth(
                        rawValue,
                        row,
                        col,
                        procMsg,
                        mockCorrectionHelper,
                        floorFilter);

                    if (rawValue == DepthImageConstants.FloorValue ||
                        rawValue == DepthImageConstants.CeilingValue)
                    {
                        Assert.AreEqual(
                            correctedValue,
                            transformedValue,
                            string.Format(
                            "Pixel {0}-{1} has changed value during transformation",
                               col,
                               row));
                    }
                    else if (rawValue < 0 || rawValue == procMsg.Parameters.UnknownDepth)
                    {
                        Assert.AreEqual(
                            correctedValue,
                            procMsg.Parameters.UnknownDepth,
                            string.Format(
                            "KinectProcessedDepth image pixel is not clamped to NoReadingValue ({0}) for input pixel value {1} but has value {2}",
                             procMsg.Parameters.UnknownDepth,
                                 rawValue,
                                 correctedValue));
                    }
                    else
                    {
                        Assert.AreEqual(
                            correctedValue,
                            transformedValue,
                            string.Format(
                            "KinectProcessedDepth image pixel {0} is not equal to the corrected value.",
                               correctedValue));
                    }
                }
            }
        }

        /// <summary>
        /// Here we apply the same logic to correction that KinectMessageProcessorUtilities::TransformDepthImage should have applied
        /// to verify we've done the right thing. 
        /// </summary>
        /// <param name="depth">Depth to correct</param>
        /// <param name="row">The row of depth pixel</param>
        /// <param name="col">The column of depth pixel</param>
        /// <param name="procMsg">Transformed or processed image</param>
        /// <param name="mockCorrectionHelper">Mock correction helper</param>
        /// <param name="floorFilter">Mock floor filter</param>
        /// <returns>Corrected depth</returns>
        private short GetCorrectedDepth(
            short depth, 
            int row, 
            int col,
            DepthCameraAgentMessage<KinectDepthCameraParameters> procMsg,
            MockCalibrationFileHelper mockCorrectionHelper,
            MockFloorFilter floorFilter)
        {
            double[][] mockCorrectionArray = mockCorrectionHelper.ImportTable("dummy name");

            int bucketRow = row / (procMsg.ImageFrame.Width / mockCorrectionHelper.BucketColumns);
            int bucketColumn = col / (procMsg.ImageFrame.Height / mockCorrectionHelper.BucketRows);

            int bucketindex = bucketRow * mockCorrectionHelper.BucketColumns + bucketColumn;

            double correctedDepth = floorFilter.FilterPlanePixels(depth, row, col);

            if (correctedDepth > 0)
            {
                correctedDepth = mockCorrectionArray[bucketindex][0] * depth + mockCorrectionArray[bucketindex][1];
            }

            if (correctedDepth == DepthImageConstants.FloorValue)
            {
                return DepthImageConstants.FloorValue;
            }
            else if (correctedDepth == DepthImageConstants.CeilingValue)
            {
                return DepthImageConstants.CeilingValue;
            }
            else if (correctedDepth <= 0)
            {
                return procMsg.Parameters.UnknownDepth;
            }
            else
            {
                return (short)Math.Round(correctedDepth);
            }
        }

        /// <summary>
        /// Checks that 2 doubles are with in a small tolerance of each other
        /// </summary>
        /// <param name="v1">First value</param>
        /// <param name="v2">Second value</param>
        /// <returns>True if Abs(v1-v2) less than tolerance</returns>
        private bool IsDoubleEqual(double v1, double v2)
        {
            return Math.Abs(v1 - v2) < MathConstants.ErrorEpsilon;
        }
    }
}
